# -*- coding: utf-8 -*-
"""
Created on Fri Dec 24 15:33:31 2021

@author: wulong
"""
import numpy as np
from casadi import *
from Long_term_formulate_Opt import *
from Obtain_results import *

def L_MPC(initial_state, state_bnds_lo, state_bnds_up, 
          input_cont_guess, input_cont_bnds_lo, input_cont_bnds_up, 
          input_cont_lbg, input_cont_ubg, 
          input_bin_guess, input_bin_bnds_lo, input_bin_bnds_up, 
          distb_bnds, 
          output_guess, 
          output_setpnts, output_setpnts_lo, output_setpnts_up, 
          alpha, alpha_slack, pred_horzn, i):
    
    initial_state_new = np.array([initial_state[16], initial_state[18], initial_state[22]])
    state_guess_new = initial_state_new
    input_cont_guess_new = np.array([input_cont_guess[0], input_cont_guess[1], 
                                     input_cont_guess[3], input_cont_guess[5], 0])
    distb_bnds_new = distb_bnds[i:i+pred_horzn, :]
    output_setpnts_new = output_setpnts[i:i+pred_horzn]
    output_setpnts_lo_new = output_setpnts_lo[i:i+pred_horzn]
    output_setpnts_up_new = output_setpnts_up[i:i+pred_horzn]
    
    f_opt = formulate_opt_l(initial_state_new, state_guess_new, state_bnds_lo, state_bnds_up, 
                            input_cont_guess_new, input_cont_bnds_lo, input_cont_bnds_up, 
                            input_cont_lbg, input_cont_ubg, 
                            input_bin_guess, input_bin_bnds_lo, input_bin_bnds_up, 
                            distb_bnds_new,
                            output_guess, 
                            output_setpnts_new, output_setpnts_lo_new, output_setpnts_up_new, 
                            alpha, alpha_slack, pred_horzn)
    
    s_opt = solve_opt_l(*f_opt)
    
    r_opt = get_results_l(s_opt, pred_horzn)
    
    return r_opt
    